// Debug drawer implementation


// Local
#include "bulletdebugdrawer.h"
#include "simutils.h"

void
BulletDebugDrawer::drawLine(const btVector3& from,const btVector3& to,const btVector3& color)
{
   /*  glColor3f(color.x(), color.y(), color.z());
     glBegin(GL_LINES);
             glVertex3f(from.x(), from.y(), from.z());
             glVertex3f(to.x(), to.y(), to.z());
     glEnd();*/

    mLineVertices << QVector3D(BT3ARGS(from)) << QVector3D(BT3ARGS(to));
    mLineColors << QVector3D(BT3ARGS(color)) << QVector3D(BT3ARGS(color));

    //printf("Line from (%f, %f, %f) to (%f, %f, %f)\n", BT3ARGS(from), BT3ARGS(to));

}

/*
 * Called while drawing cube shape
 * Does transform multiplication on cpu so not efficient. Getting opengl matrix and doing transform
 * mul on gpu is better supposedly, though computing the OpenGL matrix may take up cpu anyway ?
 * Look at GL_ShapeDrawer to see how to use the OpenGL matrix as the model matrix.
 */
void
BulletDebugDrawer::drawBox(const btVector3& bbMin, const btVector3& bbMax, const btTransform& trans, const btVector3& color)
{
/*    drawLine(trans * btVector3(bbMin[0], bbMin[1], bbMin[2]), trans * btVector3(bbMax[0], bbMin[1], bbMin[2]), color);
    drawLine(trans * btVector3(bbMax[0], bbMin[1], bbMin[2]), trans * btVector3(bbMax[0], bbMax[1], bbMin[2]), color);
    drawLine(trans * btVector3(bbMax[0], bbMax[1], bbMin[2]), trans * btVector3(bbMin[0], bbMax[1], bbMin[2]), color);
    drawLine(trans * btVector3(bbMin[0], bbMax[1], bbMin[2]), trans * btVector3(bbMin[0], bbMin[1], bbMin[2]), color);
    drawLine(trans * btVector3(bbMin[0], bbMin[1], bbMin[2]), trans * btVector3(bbMin[0], bbMin[1], bbMax[2]), color);
    drawLine(trans * btVector3(bbMax[0], bbMin[1], bbMin[2]), trans * btVector3(bbMax[0], bbMin[1], bbMax[2]), color);
    drawLine(trans * btVector3(bbMax[0], bbMax[1], bbMin[2]), trans * btVector3(bbMax[0], bbMax[1], bbMax[2]), color);
    drawLine(trans * btVector3(bbMin[0], bbMax[1], bbMin[2]), trans * btVector3(bbMin[0], bbMax[1], bbMax[2]), color);
    drawLine(trans * btVector3(bbMin[0], bbMin[1], bbMax[2]), trans * btVector3(bbMax[0], bbMin[1], bbMax[2]), color);
    drawLine(trans * btVector3(bbMax[0], bbMin[1], bbMax[2]), trans * btVector3(bbMax[0], bbMax[1], bbMax[2]), color);
    drawLine(trans * btVector3(bbMax[0], bbMax[1], bbMax[2]), trans * btVector3(bbMin[0], bbMax[1], bbMax[2]), color);
    drawLine(trans * btVector3(bbMin[0], bbMax[1], bbMax[2]), trans * btVector3(bbMin[0], bbMin[1], bbMax[2]), color);*/

    // Triangle 1
    mTriangleVertices << QVector3D(BT3ARGS(trans * btVector3(bbMin[0], bbMax[1], bbMin[2])))
                      << QVector3D(BT3ARGS(trans * btVector3(bbMin[0], bbMin[1], bbMin[2])))
                      << QVector3D(BT3ARGS(trans * btVector3(bbMin[0], bbMax[1], bbMax[2])))

                      << QVector3D(BT3ARGS(trans * btVector3(bbMin[0], bbMax[1], bbMax[2])))
                      << QVector3D(BT3ARGS(trans * btVector3(bbMin[0], bbMin[1], bbMin[2])))
                      << QVector3D(BT3ARGS(trans * btVector3(bbMin[0], bbMin[1], bbMax[2])))

                      << QVector3D(BT3ARGS(trans * btVector3(bbMin[0], bbMax[1], bbMax[2])))
                      << QVector3D(BT3ARGS(trans * btVector3(bbMin[0], bbMin[1], bbMax[2])))
                      << QVector3D(BT3ARGS(trans * btVector3(bbMax[0], bbMax[1], bbMax[2])))

                      << QVector3D(BT3ARGS(trans * btVector3(bbMax[0], bbMax[1], bbMax[2])))
                      << QVector3D(BT3ARGS(trans * btVector3(bbMin[0], bbMin[1], bbMax[2])))
                      << QVector3D(BT3ARGS(trans * btVector3(bbMax[0], bbMin[1], bbMax[2])))
//----------
                      << QVector3D(BT3ARGS(trans * btVector3(bbMax[0], bbMax[1], bbMax[2])))
                      << QVector3D(BT3ARGS(trans * btVector3(bbMax[0], bbMin[1], bbMax[2])))
                      << QVector3D(BT3ARGS(trans * btVector3(bbMax[0], bbMin[1], bbMin[2])))

					  << QVector3D(BT3ARGS(trans * btVector3(bbMax[0], bbMin[1], bbMin[2])))
					  << QVector3D(BT3ARGS(trans * btVector3(bbMax[0], bbMax[1], bbMin[2])))
					  << QVector3D(BT3ARGS(trans * btVector3(bbMax[0], bbMax[1], bbMax[2])))

					  << QVector3D(BT3ARGS(trans * btVector3(bbMax[0], bbMax[1], bbMin[2])))
					  << QVector3D(BT3ARGS(trans * btVector3(bbMax[0], bbMin[1], bbMin[2])))
					  << QVector3D(BT3ARGS(trans * btVector3(bbMin[0], bbMin[1], bbMin[2])))

					  << QVector3D(BT3ARGS(trans * btVector3(bbMin[0], bbMin[1], bbMin[2])))
					  << QVector3D(BT3ARGS(trans * btVector3(bbMin[0], bbMax[1], bbMin[2])))
					  << QVector3D(BT3ARGS(trans * btVector3(bbMax[0], bbMax[1], bbMin[2])))
//------------
    				  << QVector3D(BT3ARGS(trans * btVector3(bbMin[0], bbMax[1], bbMin[2])))  // top face
                      << QVector3D(BT3ARGS(trans * btVector3(bbMax[0], bbMax[1], bbMax[2])))
                      << QVector3D(BT3ARGS(trans * btVector3(bbMax[0], bbMax[1], bbMin[2])))

   					  << QVector3D(BT3ARGS(trans * btVector3(bbMin[0], bbMax[1], bbMin[2])))
   					  << QVector3D(BT3ARGS(trans * btVector3(bbMin[0], bbMax[1], bbMax[2])))
   					  << QVector3D(BT3ARGS(trans * btVector3(bbMax[0], bbMax[1], bbMax[2])))

   					  << QVector3D(BT3ARGS(trans * btVector3(bbMax[0], bbMin[1], bbMin[2]))) // bottom face
   					  << QVector3D(BT3ARGS(trans * btVector3(bbMax[0], bbMin[1], bbMax[2])))
   					  << QVector3D(BT3ARGS(trans * btVector3(bbMin[0], bbMin[1], bbMin[2])))

   					  << QVector3D(BT3ARGS(trans * btVector3(bbMin[0], bbMin[1], bbMin[2])))
   					  << QVector3D(BT3ARGS(trans * btVector3(bbMax[0], bbMin[1], bbMax[2])))
   					  << QVector3D(BT3ARGS(trans * btVector3(bbMin[0], bbMin[1], bbMax[2])));



    mTriangleColors << QVector3D(BT3ARGS(btVector3(0,0,1)))
					<< QVector3D(BT3ARGS(btVector3(0,0,1)))
					<< QVector3D(BT3ARGS(btVector3(0,0,1)))

       	            << QVector3D(BT3ARGS(btVector3(0,0,1)))
				    << QVector3D(BT3ARGS(btVector3(0,0,1)))
				    << QVector3D(BT3ARGS(btVector3(0,0,1)))

					<< QVector3D(BT3ARGS(btVector3(1,0,1)))
					<< QVector3D(BT3ARGS(btVector3(1,0,1)))
					<< QVector3D(BT3ARGS(btVector3(1,0,1)))

					<< QVector3D(BT3ARGS(btVector3(1,0,1)))
					<< QVector3D(BT3ARGS(btVector3(1,0,1)))
					<< QVector3D(BT3ARGS(btVector3(1,0,1)))


                    << QVector3D(BT3ARGS(btVector3(1,1,0)))
                    << QVector3D(BT3ARGS(btVector3(1,1,0)))
                    << QVector3D(BT3ARGS(btVector3(1,1,0)))

                    << QVector3D(BT3ARGS(btVector3(1,1,0)))
                    << QVector3D(BT3ARGS(btVector3(1,1,0)))
                    << QVector3D(BT3ARGS(btVector3(1,1,0)))

                    << QVector3D(BT3ARGS(btVector3(0,1,1)))
                    << QVector3D(BT3ARGS(btVector3(0,1,1)))
                    << QVector3D(BT3ARGS(btVector3(0,1,1)))

                    << QVector3D(BT3ARGS(btVector3(0,1,1)))
                    << QVector3D(BT3ARGS(btVector3(0,1,1)))
                    << QVector3D(BT3ARGS(btVector3(0,1,1)))

                    << QVector3D(BT3ARGS(btVector3(1,0,0)))
                    << QVector3D(BT3ARGS(btVector3(1,0,0)))
                    << QVector3D(BT3ARGS(btVector3(1,0,0)))

                    << QVector3D(BT3ARGS(btVector3(1,0,0)))
                    << QVector3D(BT3ARGS(btVector3(1,0,0)))
                    << QVector3D(BT3ARGS(btVector3(1,0,0)))

                    << QVector3D(BT3ARGS(btVector3(0,1,0)))
					<< QVector3D(BT3ARGS(btVector3(0,1,0)))
					<< QVector3D(BT3ARGS(btVector3(0,1,0)))

					<< QVector3D(BT3ARGS(btVector3(0,1,0)))
					<< QVector3D(BT3ARGS(btVector3(0,1,0)))
					<< QVector3D(BT3ARGS(btVector3(0,1,0)));



}

/*
 * Called while drawing sphere collision object - rasterize this !
 */
/*
void
BulletDebugDrawer::drawSphere(btScalar radius, const btTransform& transform, const btVector3& color)
{
    printf("Sphere 1");
    btIDebugDraw::drawSphere(radius, transform, color);

}
*/


